/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
typedef struct
{
    float A11;
    float A12;
    float A21;
    float A22;
} Two_Dim_Matrix;

typedef struct
{
    float position;
    float velocity;
} status_struct;
typedef struct
{
    float w1;
    float w2;
} status_error_struct;
typedef struct
{
    float v1;
    float v2;
} mesurement_error_struct;

status_error_struct process_error[30];
mesurement_error_struct measurement_error[30];
status_struct x[30], z[30], x_hat[30], x_hat_predict[30];
Two_Dim_Matrix Q, R, A, H, E, P_k[30], P_k_predict[30], K_k[30];

/**
 * @brief
 * |P|=|1 1||X1k1|
 * |v|=|0 1||X2k-1|
 *
 */
void status_para_init(void)
{
    for (int i = 0; i < 30; i++)
    {
        process_error[i].w1 = 0.01;
        process_error[i].w2 = 0.01;
    }
    x[0].position = 0;
    x[0].velocity = 1;
    //printf("x Vel \n");
    for (int i = 1; i < 30; i++)
    {
        x[i].position = A.A11 * x[i - 1].position + A.A12 * x[i - 1].velocity + process_error[i - 1].w1;
        x[i].velocity = A.A22 * x[i - 1].velocity + process_error[i - 1].w2;
        //printf("%f \n", x[i].velocity);
    }
}
void measurement_para_init(void)
{
    for (int i = 0; i < 30; i++)
    {
        measurement_error[i].v1 = 0.01;
        measurement_error[i].v2 = 0.01;
    }
    //printf("z Vel \n");
    for (int i = 0; i < 30; i++)
    {
        z[i].position = H.A11 * x[i].position + H.A12 * x[i - 1].velocity + measurement_error[i].v1;
        z[i].velocity = H.A22 * x[i].velocity + measurement_error[i].v2;
        //printf("%f \n", z[i].velocity);
    }
}
void two_dim_matrix_para_init(void)
{
    /**Q matrix */
    Q.A11 = 0.1;
    Q.A12 = 0.0;
    Q.A21 = 0.0;
    Q.A22 = 0.1;
    /**R matrix */
    R.A11 = 0.1;
    R.A12 = 0.0;
    R.A21 = 0.0;
    R.A22 = 0.1;
    /**A matrix */
    A.A11 = 1.0;
    A.A12 = 1.0;
    A.A21 = 0.0;
    A.A22 = 1.0;
    /**H matrix */
    H.A11 = 1.0;
    H.A12 = 0.0;
    H.A21 = 0.0;
    H.A22 = 1.0;
    /**E matrix */
    E.A11 = 1.0;
    E.A12 = 0.0;
    E.A21 = 0.0;
    E.A22 = 1.0;
    P_k[0].A11 = 1.0;
    P_k[0].A12 = 0.0;
    P_k[0].A21 = 0.0;
    P_k[0].A22 = 1.0;
}

Two_Dim_Matrix two_dim_matrix_transpose(Two_Dim_Matrix in)
{
    Two_Dim_Matrix result;
    result.A11 = in.A11;
    result.A12 = in.A21;
    result.A21 = in.A12;
    result.A22 = in.A22;
    return result;
}
Two_Dim_Matrix two_dim_matrix_Congruent(Two_Dim_Matrix in)
{
    Two_Dim_Matrix result;
    result.A11 = in.A22;
    result.A12 = in.A12 * (-1);
    result.A21 = in.A21 * (-1);
    result.A22 = in.A11;
    return result;
}

Two_Dim_Matrix two_dim_matrix_multiply(Two_Dim_Matrix in1, Two_Dim_Matrix in2)
{
    Two_Dim_Matrix result;
    result.A11 = in1.A11 * in2.A11 + in1.A12 * in2.A21;
    result.A12 = in1.A11 * in2.A12 + in1.A12 * in2.A22;
    result.A21 = in1.A21 * in2.A11 + in1.A22 * in2.A21;
    result.A22 = in1.A21 * in2.A12 + in1.A22 * in2.A22;
    return result;
}
Two_Dim_Matrix two_dim_matrix_sum(Two_Dim_Matrix in1, Two_Dim_Matrix in2)
{
    Two_Dim_Matrix result;
    result.A11 = in1.A11 + in2.A11;
    result.A12 = in1.A12 + in2.A12;
    result.A21 = in1.A21 + in2.A21;
    result.A22 = in1.A22 + in2.A22;
    return result;
}
Two_Dim_Matrix two_dim_matrix_reduce(Two_Dim_Matrix in1, Two_Dim_Matrix in2)
{
    Two_Dim_Matrix result;
    result.A11 = in1.A11 - in2.A11;
    result.A12 = in1.A12 - in2.A12;
    result.A21 = in1.A21 - in2.A21;
    result.A22 = in1.A22 - in2.A22;
    return result;
}
Two_Dim_Matrix two_dim_matrix_inverse(Two_Dim_Matrix in)
{
    Two_Dim_Matrix result;
    float A;
    A = in.A11 * in.A22 - in.A12 * in.A21;
    A = 1.0 / A;
    result = two_dim_matrix_Congruent(in);
    result.A11 = result.A11 * A;
    result.A12 = result.A12 * A;
    result.A21 = result.A21 * A;
    result.A22 = result.A22 * A;
    return result;
}
void kalman_calation(void)
{
    Two_Dim_Matrix buffer;
    x_hat[0].position = 0.0f;
    x_hat[0].velocity = 1.0f;
    for (int i = 1; i < 30; i++)
    {
        x_hat_predict[i].position = A.A11 * x_hat[i - 1].position + A.A12 * x_hat[i - 1].velocity;
        x_hat_predict[i].velocity = A.A22 * x_hat[i - 1].velocity;
        buffer = two_dim_matrix_multiply(A, P_k[i - 1]);
        buffer = two_dim_matrix_multiply(buffer, two_dim_matrix_transpose(A));
        P_k_predict[i] = two_dim_matrix_sum(buffer, Q);
        buffer = two_dim_matrix_sum(P_k_predict[i], R);
        buffer = two_dim_matrix_inverse(buffer);
        K_k[i] = two_dim_matrix_multiply(P_k_predict[i], buffer);
        x_hat[i].position = x_hat_predict[i].position + K_k[i].A11 * (z[i].position - x_hat_predict[i].position)
                                                      + K_k[i].A12 * (z[i].velocity - x_hat_predict[i].velocity);
        x_hat[i].velocity = x_hat_predict[i].velocity + K_k[i].A21 * (z[i].position - x_hat_predict[i].position)
                                                      + K_k[i].A22 * (z[i].velocity - x_hat_predict[i].velocity);
        P_k[i]=two_dim_matrix_multiply(two_dim_matrix_reduce(E,K_k[i]),P_k_predict[i]);

    }
}
void main(void)
{
    two_dim_matrix_para_init();
    status_para_init();
    kalman_calation();
    printf("X hat prediction\n");
    for(int i=0;i<30;i++)
    {
        printf("%f ",x_hat_predict[i].velocity);

    }
    printf("X hat \n");
    for(int i=0;i<30;i++)
    {
        printf("%f ", x_hat)
    }

}
